import numpy as np
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
import math
class camera:
    def __init__(self,IMAX,DIST,R,T):
        self.imtx=np.array(IMAX)#内参矩阵
        self.dist=np.array(DIST)#畸变参数
        self.newcameramtx, self.roi=cv2.getOptimalNewCameraMatrix(self.imtx,self.dist,(1920,1080),0,(1920,1080))
        self.fx=self.imtx[0,0]
        self.fy=self.imtx[1,1]
        self.cx=self.imtx[0,2]
        self.cy=self.imtx[1,2]
        self.f=(self.fx+self.fy)/2
        self.r,self.dr=cv2.Rodrigues(np.array(R))
        self.t=np.array(T)*19.3#格子大小mm
        
    def undistort(self,img):#去畸变 不过这个相机畸变量挺小的 标定出来的畸变参数似乎是假的
        #print(self.imtx)
        h,w = img.shape[:2]
        newcameramtx, roi=cv2.getOptimalNewCameraMatrix(self.imtx,self.dist,(w,h),0,(w,h))
        dst = cv2.undistort(img, self.imtx, self.dist, None, self.newcameramtx)
        x,y,w,h = self.roi
        dst = dst[y:y+h, x:x+w]
        #print("ok")
        return dst
'''
L=camera(IMAX=[[1.56946348e+03,0.00000000e+00,9.65258096e+02]#imgL_point_424
             ,[0.00000000e+00,1.56793866e+03,4.95980421e+02]
             ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-7.30986552e-02 ,3.99374120e-01 ,9.28169866e-04 ,2.82906248e-04 ,-9.73325096e-01])
'''
L=camera(IMAX=[[1.57366985e+03 ,0.00000000e+00 ,9.42156018e+02]#imgL_cross_424
 ,[0.00000000e+00 ,1.56435543e+03 ,5.14905625e+02]
 ,[0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]],
         DIST=[-0.03020629 ,-0.01574204 , 0.00609551 ,-0.00025611  ,0.10228617],
         R=[-0.02579014,-0.72471525,-3.01730134],T=[ 8.38819527,2.42251929,19.69617263])
'''
R=camera(IMAX=[[1.57018321e+03,0.00000000e+00,9.56921749e+02]#imgR_point_424
             ,[0.00000000e+00,1.56994540e+03,4.95969393e+02]
             ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-0.07213587 ,0.29454981,-0.00159414 ,0.00068262,-0.35742506])
'''
R=camera(IMAX= [[1.57534870e+03,0.00000000e+00,9.12358094e+02]#imgR_cross_424
 ,[0.00000000e+00,1.57159939e+03,5.11227097e+02]
 ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-0.0651569   ,0.36046137 , 0.00704255 ,-0.00412844 ,-0.88801585],
         R=[-0.01221774,-0.72624792,-3.01423727],T=[ 2.20273285,2.46638729,19.73086734])

#print(np.linalg.norm(L.t-R.t))
#R,T,E,F=cv2.stereoCalibrate()
